#include "ros/ros.h"
#include "std_msgs/String.h"
#include "mbot_linux_serial.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/Int32MultiArray.h"

// 定义变量用于测试发送和接收数据
double testSend1 = 5555.0;
double testSend2 = 2222.0;
unsigned char testSend3 = 0x07;
int joint[6] = {7777, 6666, 5555, 4444, 3333, 2222};

float pos[5] = {0};
unsigned char checkFlag = 0x00;
unsigned char statusInfo = 0x00;
unsigned char errorInfo = 0x00;
int speedValues[5] = {0};
int positionValues[5] = {0};

ros::Publisher speed_pub; // 声明发布者

// 回调函数：接收和处理 "/joint_states" 话题的数据
void Callback(const sensor_msgs::JointStateConstPtr& msg) {
int len = msg->position.size();
for (int j = 0; j < len && j < 5; j++) {
pos[j] = msg->position[j];
}
if (len >= 5) {
// ROS_INFO("pos,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4]);
}

for (int i = 0; i < 5; i++) {
joint[i] = pos[i] * 10000;
}

writeSpeed(joint, testSend3);
}

// 函数：读取速度和位置信息并发布 speedValues
void readSpeedAndDisplay() {
bool success = readSpeed(checkFlag, statusInfo, errorInfo, speedValues, positionValues);

if (success) {
// ROS_INFO("Speed: %d, %d, %d, %d, %d", speedValues[0], speedValues[1], speedValues[2], speedValues[3], speedValues[4]);
// ROS_INFO("positon: %d, %d, %d, %d, %d", positionValues[0], positionValues[1], positionValues[2], positionValues[3], positionValues[4]);
ROS_INFO("data: %d, %d, %d, %d, %d, %d, %d, %d, %d, %d", speedValues[0], speedValues[1], speedValues[2], speedValues[3], speedValues[4], positionValues[0], positionValues[1], positionValues[2], positionValues[3], positionValues[4]);

// 创建消息并发布
std_msgs::Int32MultiArray speed_msg;
speed_msg.data.insert(speed_msg.data.end(), speedValues, speedValues + 5);
speed_pub.publish(speed_msg);

} else {
// ROS_WARN("Failed to read data.");
}
}

int main(int argc, char **argv) {
ros::init(argc, argv, "communication_node");
ros::NodeHandle nh;

Serial_Init();

// 设置订阅 "/joint_states" 话题
ros::Subscriber sub = nh.subscribe("/joint_states", 10000, Callback);

// 设置发布 "/speed_values" 话题
speed_pub = nh.advertise<std_msgs::Int32MultiArray>("/speed_values", 1000);

ros::Rate loop_rate(100);

while (ros::ok()) {
readSpeedAndDisplay();
ros::spinOnce(); 
loop_rate.sleep();
 }

return 0; 
}